#include "FinalAlign.h"
FinalAlign::FinalAlign(int position,VisioneController *vision,double *beta):ArAction("finale", "Segna") {
	this->position=position;
	this->vision=vision;
	this->beta=beta;
	allineato = false;
	segnato = false;
}
FinalAlign::~FinalAlign(){

}
ArActionDesired* FinalAlign::fire(ArActionDesired currentDesired){

	myDesired.reset();


	int range = 45;
	unsigned short ballD = vision->getProfonditaPalla();
	unsigned short goalD = vision->getProfonditaPorta();
	int angoloPalla = vision->getAngoloPalla();

	if(angoloPalla>=-range && angoloPalla<=range && ballD!=NULL){
		if(allineato==false){
			allineato = true;
			myRobot->setVel(800);
			myRobot->move(1500);
			std::cout <<"segna"<<endl;
		}
	}else if(allineato==false){
		double toRotate = 5;
		if(position>0){
			myRobot->setDeltaHeading(-toRotate);
		}else if(position<0){
			myRobot->setDeltaHeading(toRotate);
		}

	}

	//}


	return &myDesired;
}
